%Find Fundamental matrix with Gold Standard Method
%Based on Hartley and Zisserman's Method

function Fgs = gsFundamental(Flin , corres1, corres2)

%Find epipolar prime from linear F
[Uep,Dep,Vep] = svd(Flin);
ep = Uep(:,3);
epx = [0         -ep(3,1)        ep(2,1)    ;
       ep(3,1)      0           -ep(1,1)    ;
      -ep(2,1)    ep(1,1)          0       ];
       

%Let P1 = [ I|0 ]
PF1 = [eye(3) [0;0;0]];

%Let P2 = [ [e'] F | e' ]
PF2 = [ epx*Flin, ep ];


% Find Optimize P2 by Levenberg Marquartd
P2GS = lsqnonlin(@(h)ReprojErrGS(corres1,PF1,corres2,h),PF2);


%Calculate Fgs from P2GS
%P2GS = [M | m]

m = P2GS(:,4);
M = P2GS(:,1:3);
Fgs = CrossForm(m)*M;

%--------------------------------------------------------------------------
function Xw = TriangulationGS(x1,P1,x2,P2)

% Argument
%   x1,x2 = Point in 2D space [x1,...,xn ; y1,...,yn ; 1,...,1]
%   P1,P2 = Projection Transformation Matrix
%   F = Fundamental Matrix
%   Xw = Point in 3D space [X1,...,Xn ; Y1,...,Yn ; Z1,...,Zn ; 1,...,1]

for i=1:size(x1,2)
    %Select point
    sx1 = x1(:,i);
    sx2 = x2(:,i);
    
    %Transfrom the selected point into cross product form
    %Cx1 = CrossForm(x1_hat);
    %Cx2 = CrossForm(x2_hat);
    %Cx1 = CrossForm(sx1);
    %Cx2 = CrossForm(sx2);
    
    %Set A matric, and find SDV(A)
    A1 = sx1(1,1).*P1(3,:) - P1(1,:);
    A2 = sx1(2,1).*P1(3,:) - P1(2,:);
    A3 = sx2(1,1).*P2(3,:) - P2(1,:);
    A4 = sx2(2,1).*P2(3,:) - P2(2,:);
    
    %Set A matric, and find SDV(A)
    A = [A1;A2;A3;A4];
    
    [U,D,V] = svd(A);
    
    %Point in 3D space is the last column of V
    X_temp = V(:,4);
    X_temp = X_temp ./ repmat(X_temp(4,1),4,1);
    
    Xw(:,i) = X_temp;
    
end

%------------------------------------------------------------------
function Cx = CrossForm(a)

 Cx = [ 0       -a(3,1)       a(2,1) ; 
        a(3,1)     0         -a(1,1) ; 
       -a(2,1)   a(1,1)          0  ];
        
%-------------------------------------------------------------------

function REGS = ReprojErrGS(corres1,PF1,corres2,PF2)

%Find estimated 3D point by Triangulation method
XwEst = TriangulationGS(corres1,PF1,corres2,PF2);

%Reprojection Back to the image
x1hat = PF1*XwEst;
x1hat = x1hat ./ repmat(x1hat(3,:),3,1);

x2hat = PF2*XwEst;
x2hat = x2hat ./ repmat(x2hat(3,:),3,1);

%Find root mean squared distance error
dist = ((corres1 - x1hat).*(corres1 - x1hat))  +  ((corres2 - x2hat).*(corres2 - x2hat));
REGS = sqrt(sum(sum(dist)) / size(corres1,2));
















